clear all;
close all;


m=1.12;g=9.81;
Ix=0.0119;Iy=0.0119;Iz=0.0223;
b=0.00000773213;d=0.000000127513;Jr=0.00085;l=0.23;
% kp1=1.2;kp2=1.2;kp3=1.2;kp4=5.62;
% kint1=3.1;kint2=3.1;kint3=3.1;kint4=7.2;
% kd1=0.08;kd2=0.08;kd3=0.03;kd4=1.0;
% beta1=1.0;beta2=1.0;beta3=3;beta4=2.0;
% lamb1=1.2;lamb2=1.2;lamb3=4;lamb4=1.01;
gama1=0.72;gama2=0.72;gama3=0.21;gama4=2.2;
OHM=1000;

Ixx=0.0119;Iyy=0.0119;Izz=0.0223;
a1=(Iyy-Izz)/Ixx;a2=Jr/Ixx;
a3=(Izz-Ixx)/Iyy;a4=-Jr/Iyy;
a5=(Ixx-Iyy)/Izz;b1=l/Ixx;
b2=l/Iyy;b3=l/Izz;


p1=5;q1=7;p2=3;q2=5;p=3;q=5;
ax=2.4;bx=1;ay=2.2;by=0.8;az=3.4;bz=2.2;
a11=10;a21=22;b11=6;
a12=10;a22=30;b22=10;
a13=6;a23=12;b33=6;
ll=1;
% kr1=4.6;kr2=4.6;kr3=7.4;
% ko1=0.86;ko2=0.86;ko3=0.86;
% k2=14.3;fi1=1.5;J=0.01;
% kf1=0.05;kf2=0.05;kf3=0.1;k3=0.1;k2=1.43;

Qk=[1;0.001;1;0.001;0.5;0.001;5;0;0;0;0;0];% ode45不能直接用矩阵，得变成向量进行操作；依次是phi,th,fi,h

T=0.001;
for k=1:1:30000
  
    time(k)=k*T;

    phid(k)=0;
    thd(k)=0;
    fid(k)=0;
    hd(k)=0;
    
    Qd(:,k)=[phid(k);thd(k);fid(k);hd(k)];             
     
%     if(k*T>=5)
%         dt(1)=2*cos(2*pi/3*k*T);
%         dt(2)=7+2*cos(2*pi/3*k*T);
%         dt(3)=5+2*cos(pi/2*k*T);
%         dt(4)=18+4*cos(pi/6*k*T);
%     else
        dt(1)=0;dt(2)=0;dt(3)=0;dt(4)=0;
%     end

    phi(k)=Qk(1);dphi(k)=Qk(2);th(k)=Qk(3);dth(k)=Qk(4);
    fi(k)=Qk(5);dfi(k)=Qk(6);h(k)=Qk(7);dh(k)=Qk(8);
    x(k)=Qk(9);dx(k)=Qk(10);y(k)=Qk(11);dy(k)=Qk(12);

    x1=Qk(1);x2=Qk(2);x3=Qk(3);x4=Qk(4);
    x5=Qk(5);x6=Qk(6);x7=Qk(7);x8=Qk(8);
    x9=Qk(9);x10=Qk(10);x11=Qk(11);x12=Qk(12);
    
    Q(:,k)=[phi(k);th(k);fi(k);h(k)];
    dQ(:,k)=[dphi(k);dth(k);dfi(k);dh(k)];
    
    e(:,k) = Qd(:,k)-Q(:,k);
    e1=e(1,k);e2=e(2,k);e3=e(3,k);e4=e(4,k);
    de(:,k) = -dQ(:,k);
    de1=de(1,k);de2=de(2,k);de3=de(3,k); de4=de(4,k); 

    s1 = e1 + (a11^(-1)*de1)^(q1/p1);
    s2 = e2 + (a12^(-1)*de2)^(q1/p1);
    s3 = e3 + (a13^(-1)*de3)^(q1/p1);
    s(:,k)=[s1;s2;s3];

    if(de1^(q1/p1-1)<=ll)
        ul1=sin(pi/2*de1^(q1/p1-1)/ll);
    else
        ul1=1;
    end
    if(de2^(q1/p1-1)<=ll)
        ul2=sin(pi/2*de2^(q1/p1-1)/ll);
    else
        ul2=1;
    end
    if(de3^(q1/p1-1)<=ll)
        ul3=sin(pi/2*de2^(q1/p1-1)/ll);
    else
        ul3=1;
    end

    vt1(k)=m/(cos(x1)*cos(x3))*(az*de4 + bz*de4^(p/q) + g + gama4*sign(de4));
    vt2(k)=b1^(-1)*( p1/q1*a11^( q1/p1 )*de1^(1-q1/p1)*( de1 +ul1 *(a21*s1 +b11*s1^( p2/q2 )) ) - (x4*x6*a1+x4*a2*OHM) + gama1*sign(s1));
    vt3(k)=b2^(-1)*( p1/q1*a12^( q1/p1 )*de2^(1-q1/p1)*( de2 +ul2 *(a22*s2 +b22*s2^( p2/q2 )) ) - (x2*x6*a3+x2*a4*OHM) + gama2*sign(s2));
    vt4(k)=b3^(-1)*( p1/q1*a13^( q1/p1 )*de3^(1-q1/p1)*( de3 +ul3 *(a23*s3 +b33*s3^( p2/q2 )) ) - (x2*x4*a5) + gama3*sign(s3));
    
    
    U=[vt1(k);vt2(k);vt3(k);vt4(k)];

    tSpan = [0 T];
	[tt,Qq] = ode45(@(t,x) plant(t,x,U),tSpan,Qk);
    Qk = Qq(length(Qq),:);                             
end

figure(1);
plot(time,phi,'k',time,phid,'r--');
xlabel('time(s)');ylabel('phi');
legend('φq','φd');
grid on;

figure(2);
plot(time,th,'k',time,thd,'r--');
xlabel('time(s)');ylabel('th');
legend('θq','θd');
grid on;
 
figure(3);
plot(time,fi,'k',time,fid,'r--');
xlabel('time(s)');ylabel('fi');
legend('ψq','ψd');
grid on;
 
figure(4);
plot(time,h,'k',time,hd,'r--');
xlabel('time(s)');ylabel('h');
legend('hq','hd');
grid on;

figure(5);
plot(time,vt1,'k');
xlabel('time(s)');ylabel('U1');
grid on;

figure(6);
plot(time,vt2,'k');
xlabel('time(s)');ylabel('U2');
grid on;

figure(7);
plot(time,vt3,'k');
xlabel('time(s)');ylabel('U3');
grid on;

figure(8);
plot(time,vt4,'k');
xlabel('time(s)');ylabel('U4');
grid on;